/*
  User: Yemao Luo
  Date: 2021/9/24
  Time: 22:41
*/

import java.util.ArrayList;
import java.util.Collections;
import java.util.PriorityQueue;
import java.util.Scanner;

public class Class04HW {
    public static void main(String[] arg) {
        int[][] map2d = {
                {0, 0, 0, 0, 0, 0},
                {0, 0, 1, 0, 0, 0},
                {0, 0, 1, 0, 0, 0},
                {0, 0, 1, 0, 0, 0},
                {0, 0, 1, 1, 1, 0},
                {0, 0, 0, 0, 0, 0},
        };
        Scanner sc = new Scanner(System.in);
        Point startPoint = new Point(0, 0);
        System.out.println("StartPoint");
        System.out.print("x: ");
        startPoint.x = sc.nextInt();
        System.out.print("y: ");
        startPoint.y = sc.nextInt();
        Point endPoint = new Point(0, 0);
        System.out.println("EndPoint");
        System.out.print("x: ");
        endPoint.x = sc.nextInt();
        System.out.print("y: ");
        endPoint.y = sc.nextInt();

        AStar astar = new AStar(map2d, startPoint, endPoint);
        ArrayList<Point> pointList_astar = astar.start();
        System.out.print("Astar: ");
        for (Point point : pointList_astar) {
            System.out.print("(" + point.x + "," + point.y + ") ");
        }
        System.out.println();

        Dijkstra dijkstra = new Dijkstra(map2d, startPoint, endPoint);
        ArrayList<Point> pointList_dijkstra = dijkstra.start();
        System.out.print("Dijkstra: ");
        for (Point point : pointList_dijkstra) {
            System.out.print("(" + point.x + "," + point.y + ") ");
        }
        System.out.println();

        BFS.BFS(map2d);
    }
}

class Point {
    public int x;
    public int y;

    public Point(int x, int y) {
        this.x = x;
        this.y = y;
    }

    public boolean eq(Point p) {
        return this.x == p.x && this.y == p.y;
    }
}

class Node_Astar implements Comparable<Node_Astar> {
    public Point point;
    public Node_Astar father;
    double g;
    double h;

    public Node_Astar(Point point, Point endPoint, double g) {
        this.point = point;
        this.g = g;
        this.h = Math.sqrt(Math.pow((endPoint.x - point.x), 2) + Math.pow((endPoint.y - point.y), 2));
    }

    @Override
    public int compareTo(Node_Astar o) {
        if (o == null) return -1;
        if (this.g + this.h > o.g + o.h) {
            return 1;
        } else if (this.g + this.h < o.g + o.h) {
            return -1;
        } else {
            return 0;
        }
    }
}

class AStar {
    private PriorityQueue<Node_Astar> openList = new PriorityQueue<>();
    private ArrayList<Node_Astar> closeList = new ArrayList<>();
    private int[][] map;
    private Point startPoint;
    private Point endPoint;
    private int w;
    private int h;

    public AStar(int[][] map, Point startPoint, Point endPoint) {
        this.map = map;
        this.w = map.length;
        this.h = map[0].length;

        this.startPoint = startPoint;
        this.endPoint = endPoint;
    }

    public Node_Astar pointInCloseList(Point point) {
        for (Node_Astar Node_Astar : this.closeList) {
            if (Node_Astar.point.eq(point)) {
                return Node_Astar;
            }
        }
        return null;
    }

    public Node_Astar pointInOpenList(Point point) {
        for (Node_Astar Node_Astar : this.openList) {
            if (Node_Astar.point.eq(point)) {
                return Node_Astar;
            }
        }
        return null;
    }

    private void searchNear(Node_Astar minNode_Astar, int offsetX, int offsetY) {
        if (minNode_Astar.point.x + offsetX < 0 || minNode_Astar.point.x + offsetX >= this.w || minNode_Astar.point.y + offsetY < 0 || minNode_Astar.point.y + offsetY >= this.h) {
            return;
        }
        if (this.map[minNode_Astar.point.x + offsetX][minNode_Astar.point.y + offsetY] != 0) {
            return;
        }
        Point currentPoint = new Point(minNode_Astar.point.x + offsetX, minNode_Astar.point.y + offsetY);
        if (this.pointInCloseList(currentPoint) != null) {
            return;
        }
        int step;
        if (offsetX == 0 || offsetY == 0) {
            step = 10;
        } else {
            step = 14;
        }
        Node_Astar currentNode_Astar = this.pointInOpenList(currentPoint);
        if (currentNode_Astar == null) {
            currentNode_Astar = new Node_Astar(currentPoint, this.endPoint, minNode_Astar.g + step);
            currentNode_Astar.father = minNode_Astar;
            this.openList.add(currentNode_Astar);
            return;
        }
        if (minNode_Astar.g + step < currentNode_Astar.g) {
            currentNode_Astar.g = minNode_Astar.g + step;
            currentNode_Astar.father = minNode_Astar;
        }
    }

    public ArrayList<Point> start() {
        ArrayList<Point> pointList = new ArrayList<>();

        if (this.map[this.endPoint.x][this.endPoint.y] == 1) {
            return null;
        }

        Node_Astar startNode_Astar = new Node_Astar(this.startPoint, this.endPoint, 0);
        this.openList.add(startNode_Astar);

        while (true) {
            Node_Astar minNode_Astar = this.openList.poll();
            this.closeList.add(minNode_Astar);
            this.searchNear(minNode_Astar, 0, -1);
            this.searchNear(minNode_Astar, 0, 1);
            this.searchNear(minNode_Astar, -1, 0);
            this.searchNear(minNode_Astar, 1, 0);
            this.searchNear(minNode_Astar, -1, -1);
            this.searchNear(minNode_Astar, 1, 1);
            this.searchNear(minNode_Astar, -1, 1);
            this.searchNear(minNode_Astar, 1, -1);

            Node_Astar Node_Astar = this.pointInCloseList(this.endPoint);
            if (Node_Astar != null) {
                while (true) {
                    if (Node_Astar.father != null) {
                        pointList.add(Node_Astar.point);
                        Node_Astar = Node_Astar.father;
                    } else {
                        pointList.add(startPoint);
                        Collections.reverse(pointList);
                        return pointList;
                    }
                }
            }

            if (this.openList.size() == 0) {
                return null;
            }
        }
    }
}

class Node_Dijkstra implements Comparable<Node_Dijkstra>{
    public Point point;
    public Node_Dijkstra father;
    double g;
    double h;

    public Node_Dijkstra(Point point, Point endPoint, double g) {
        this.point = point;
        this.g = g;
        this.h = 0;
    }

    @Override
    public int compareTo(Node_Dijkstra o) {
        if (o == null) return -1;
        if (this.g + this.h > o.g + o.h) {
            return 1;
        } else if (this.g + this.h < o.g + o.h) {
            return -1;
        } else {
            return 0;
        }
    }
}

class Dijkstra{
    private PriorityQueue<Node_Dijkstra> openList = new PriorityQueue<>();
    private ArrayList<Node_Dijkstra> closeList = new ArrayList<>();
    private int[][] map;
    private Point startPoint;
    private Point endPoint;
    private int w;
    private int h;

    public Dijkstra(int[][] map, Point startPoint, Point endPoint) {
        this.map = map;
        this.w = map.length;
        this.h = map[0].length;

        this.startPoint = startPoint;
        this.endPoint = endPoint;
    }

    public Node_Dijkstra pointInCloseList(Point point) {
        for (Node_Dijkstra Node_Dijkstra : this.closeList) {
            if (Node_Dijkstra.point.eq(point)) {
                return Node_Dijkstra;
            }
        }
        return null;
    }

    public Node_Dijkstra pointInOpenList(Point point) {
        for (Node_Dijkstra Node_Dijkstra : this.openList) {
            if (Node_Dijkstra.point.eq(point)) {
                return Node_Dijkstra;
            }
        }
        return null;
    }

    private void searchNear(Node_Dijkstra minNode_Dijkstra, int offsetX, int offsetY) {
        if (minNode_Dijkstra.point.x + offsetX < 0 || minNode_Dijkstra.point.x + offsetX >= this.w || minNode_Dijkstra.point.y + offsetY < 0 || minNode_Dijkstra.point.y + offsetY >= this.h) {
            return;
        }
        if (this.map[minNode_Dijkstra.point.x + offsetX][minNode_Dijkstra.point.y + offsetY] != 0) {
            return;
        }
        Point currentPoint = new Point(minNode_Dijkstra.point.x + offsetX, minNode_Dijkstra.point.y + offsetY);
        if (this.pointInCloseList(currentPoint) != null) {
            return;
        }
        int step;
        if (offsetX == 0 || offsetY == 0) {
            step = 10;
        } else {
            step = 14;
        }
        Node_Dijkstra currentNode_Dijkstra = this.pointInOpenList(currentPoint);
        if (currentNode_Dijkstra == null) {
            currentNode_Dijkstra = new Node_Dijkstra(currentPoint, this.endPoint, minNode_Dijkstra.g + step);
            currentNode_Dijkstra.father = minNode_Dijkstra;
            this.openList.add(currentNode_Dijkstra);
            return;
        }
        if (minNode_Dijkstra.g + step < currentNode_Dijkstra.g) {
            currentNode_Dijkstra.g = minNode_Dijkstra.g + step;
            currentNode_Dijkstra.father = minNode_Dijkstra;
        }
    }

    public ArrayList<Point> start() {
        ArrayList<Point> pointList = new ArrayList<>();

        if (this.map[this.endPoint.x][this.endPoint.y] == 1) {
            return null;
        }

        Node_Dijkstra startNode_Dijkstra = new Node_Dijkstra(this.startPoint, this.endPoint, 0);
        this.openList.add(startNode_Dijkstra);

        while (true) {
            Node_Dijkstra minNode_Dijkstra = this.openList.poll();
            this.closeList.add(minNode_Dijkstra);
            this.searchNear(minNode_Dijkstra, 0, -1);
            this.searchNear(minNode_Dijkstra, 0, 1);
            this.searchNear(minNode_Dijkstra, -1, 0);
            this.searchNear(minNode_Dijkstra, 1, 0);
            this.searchNear(minNode_Dijkstra, -1, -1);
            this.searchNear(minNode_Dijkstra, 1, 1);
            this.searchNear(minNode_Dijkstra, -1, 1);
            this.searchNear(minNode_Dijkstra, 1, -1);

            Node_Dijkstra Node_Dijkstra = this.pointInCloseList(this.endPoint);
            if (Node_Dijkstra != null) {
                while (true) {
                    if (Node_Dijkstra.father != null) {
                        pointList.add(Node_Dijkstra.point);
                        Node_Dijkstra = Node_Dijkstra.father;
                    } else {
                        pointList.add(startPoint);
                        Collections.reverse(pointList);
                        return pointList;
                    }
                }
            }

            if (this.openList.size() == 0) {
                return null;
            }
        }
    }
}

class Node_BFS {
    public int x;
    public int y;

    @Override
    public String toString() {
        return "(" + x + "," + y + ")";
    }

    public int getX() {
        return x;
    }

    public void setX(int x) {
        this.x = x;
    }

    public int getY() {
        return y;
    }

    public void setY(int y) {
        this.y = y;
    }

    public Node_BFS(int x, int y) {
        this.x = x;
        this.y = y;
    }
}

class BFS {
    public static Boolean check(ArrayList<Node_BFS> points, Node_BFS point) {
        for (int i = 0; i < points.size(); i++) {
            if (points.get(i).x == point.x && points.get(i).y == point.y) {
                return false;
            }
        }
        return true;
    }

    public static void BFS(int[][] map) {
        Node_BFS start_point = new Node_BFS(0, 0);
        ArrayList<Node_BFS> points = new ArrayList<Node_BFS>();
        points.add(start_point);
        Node_BFS[] direction = {
                new Node_BFS(0, 1),
                new Node_BFS(1, 0),
                new Node_BFS(0, -1),
                new Node_BFS(-1, 0)
        };
        for (int i = 0; i < points.size(); i++) {
            for (int j = 0; j < direction.length; j++) {
                int x = points.get(i).x + direction[j].x;
                int y = points.get(i).y + direction[j].y;
                if (x < 0 || y < 0) {
                    continue;
                }
                Node_BFS point_tem = new Node_BFS(x, y);
                if (x < map.length && y < map[0].length && check(points, point_tem) && map[x][y] != 1) {
                    points.add(point_tem);
                }
            }
        }
        System.out.println("BFS: " + points.toString());
    }
}





